%% Events function for MATLAB ode function.
% [value, isterminal, direction] = guard_map(x, p)
%
%  description:
%   events function for the ODE integrator.  returns the parameters
%   required by the events function.
%
%
%  Arguments:
%   x          - the value of the continuous state at which the guard
%                function is to be evaluated.
%   p          - the discrete state parameters.
%
%  Return Values:
%   value      - a row matrix of values obtained by evaluating the passed
%                guards.
%   isterminal - a row matrix specifying if the guard associated with the
%                row should cause the integrator to terminate and thus
%                reset to the next domain.  In general, guards should
%                always cause the integrator to terminate, however, this
%                flag can be used to avoid phenomena such as scuffing.
%   direction  - a row matrix specifying in which direction the guard
%                associated with the row should cause the integrator to
%                terminate.  With our guards, we would like the guard
%                function to be decreasing, e.g. the foot is getting closer
%                to the ground.
%
function [value, isterminal, direction] = guard_map(t, x, d)

% integration should terminate when the system hits the guard
isterminal = 1;

if (t < .2)
    isterminal = 0;
end

% guard functions should be defined such that the zero-crossing is from a
% positive value to a negative value
direction = -1;

% return the value of the unilateral constraint
fh = @get_terrain;
jp = jointpos(x);
p_nsf = jp(1, end) + d;
value = h_sca(x) - (fh(p_nsf) - fh(d));
%value = h_sca(x);